 
#include <vxWorks.h>
#include <vsbConfig.h>
#include <intLib.h>
#include <errnoLib.h>
#include <errno.h>
#include <sioLib.h>
#include <ioLib.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <vxAtomicLib.h>
#include <semLib.h>
#include <sysLib.h>
#include <vmLib.h>
#include <stdio.h>
#include <hwif/vxBus.h>
#include <hwif/buslib/vxbFdtLib.h>
#include <taskLib.h>
#include <pipeDrv.h>      /* 管道驱动 */
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <cacheLib.h>
#include <sys/times.h>  // Add this include for struct timeval
#include "axiSrio.h"
#include "icd.h"


// 全局变量
static VXB_DEV_ID g_srioDev = NULL;
static SEM_ID g_srioSem = NULL;

static uint32_t data_size = 0;  // 全局变量，用于存储数据量;
static uint32_t * sample_buffer_addr[20];  // 定义一个全局变量来存储地址

// 数据包有效数据最大字节数限制为1000字节
#define MAX_PACKET_DATA_SIZE 1000
#define PACKET_DATA_SIZE sizeof(((DataTransferResponse *)0)->data)

/* SRIO设备函数声明 */
void axiSrioOpen(VXB_DEV_ID *ppDev, UINT32 unit);
int axiSrioClose(VXB_DEV_ID pDev);
STATUS axiSrioIntConnect(VXB_DEV_ID pDev, VOIDFUNCPTR isr);
STATUS axiSrioSend(VXB_DEV_ID pDev, const void *pData, UINT32 dataLen, UINT32 destId);
void *axiSrioGetRxAddr(VXB_DEV_ID pDev);


static void srio_receive_task(void) ;
int cnt_srio_isr = 0;
static void srio_isr() {
	cnt_srio_isr++;
    // 处理SRIO中断
    semGive(g_srioSem); 
}

// 初始化SRIO服务器
STATUS srio_server_init() { 
    g_srioSem = semBCreate(SEM_Q_PRIORITY, SEM_EMPTY);    
    if (g_srioSem == NULL) {
        return ERROR;
    }
    axiSrioOpen(&g_srioDev, 0); 
    axiSrioIntConnect(g_srioDev, srio_isr); 
    // 创建 SRIO 接收任务
     // 任务优先级设为 100，栈大小设为 4096，可根据实际情况调整
	 if (taskSpawn("tSrioReceive", 100, 0, 40960, (FUNCPTR)srio_receive_task, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0) == ERROR) {
		 printf("Failed to spawn SRIO receive task\n");
	 }
    return OK;
}

// 握手指令处理
STATUS srio_process_handshake(IcdPacket* packet) {
	int i;
    HandResponse resp = {0};
    resp.header_flag = 0x5AA55AA5;
    resp.command_type = ICD_HANDSHAKE;
    
    uint8_t slot_number = get_fpga_slot_number();
    resp.zynq_phy_addr = slot_number;
    resp.tail_flag = 0xADDEADDE;
     
    axiSrioSend(g_srioDev, &resp, sizeof(resp), 0);
    // 通过SRIO发送响应
    return OK;
}

// 模式设置指令处理
STATUS srio_process_mode_set(IcdPacket* packet) {
	int i,j;
    ModeSetResponse resp = {0};
    resp.header_flag = 0x5AA55AA5;
    resp.command_type = ICD_MODE_SET;
    resp.tail_flag = 0xADDEADDE;
    clear_group_state(0xFF);  // 清除所有20个采集组的完成状态

    // 获取槽位号并填充到物理地址
    uint8_t slot_number = get_fpga_slot_number();
    resp.zynq_phy_addr = slot_number;
    

    uint8_t time  = packet->payload[2];
    uint8_t freq  = packet->payload[3]; 
    resp.zynq_sample_duration = time;
    resp.zynq_sample_freq = freq;
    
    
    // 计算采样时长，为简化计算，提前将频率MHz转化
    uint32_t sampling_duration_s = 200+  100*time;
    // 计算数据量（单位：字节）
    data_size = (uint32_t)(sampling_duration_s * 2 * 40 * 1000);

    logMsg("Calculated data size: %u bytes\n", data_size, 0, 0, 0, 0, 0);
    
    // 假设 sample_buffer_addr 是一个包含20个指针的数组
    for (i = 0; i < 20; i++) {

        uint8_t group_byte = packet->payload[4 + i/4];  // 每组4位，所以每字节包含4个组的信息
        logMsg("Payload byte for group %d: 0x%02x\n", i, group_byte, 0, 0, 0, 0);
        uint8_t group_bit = i % 4;
        uint8_t is_enabled = (group_byte >> (group_bit * 2 + 1)) & 0x01;  // 采集使能位
        uint8_t mode = (group_byte >> (group_bit * 2)) & 0x01;    // 采集模式位

        if (!is_enabled) {
            logMsg("Group %d is disabled, skip configuration\n", i, 0, 0, 0, 0, 0);
            enable_group(i, 0);

            continue;  // 组不采集，跳过
        }
        if (is_enabled) {
            // 判断第2个payload最低位，决定是否配置自测试
            if (packet->payload[1] & 0x01) {
                // 调用自测试配置函数
                config_self_test(i, 1);
                logMsg("Self-test configuration enabled.\n", 0, 0, 0, 0, 0, 0);
            }
            else
            {
                config_self_test(i, 0);
                logMsg("Self-test configuration disabled.\n", 0, 0, 0, 0, 0, 0);
            }  
            config_sample_dur(i, sampling_duration_s *  40 * 1000);
            // 从第15字节开始，每个组对应1字节配置（组0对应索引14，组1对应索引15...）
            uint8_t edge_cfg = packet->payload[9 + i];  // payload[14]对应组0
            logMsg("Group %d edge config: 0x%02x\n", i, edge_cfg, 0, 0, 0, 0);
            // 解析高4位为边沿管脚（0-11）
            unsigned char edge_pin = (edge_cfg >> 4) & 0x0F;
            if(edge_pin > 11) {
                logMsg("Group %d edge pin out of range: %d\n", i, edge_pin, 0, 0, 0, 0);
                edge_pin = 0;  // 使用默认管脚
            }
    
            // 解析bit3为边沿类型（1=上升沿，0=下降沿）
            unsigned char rise_fall = (edge_cfg >> 3) & 0x01;
    
            // 调用配置函数（参数顺序：group, edge_pin, enable, rise_fall）

            if(mode == 0)     
            	config_edge_trigger(i, edge_pin, 1, rise_fall);
            else
                config_edge_trigger(i, edge_pin, 0, rise_fall);
            	
            logMsg("Group %d edge config: pin=%d type=%s\n", 
                    i, edge_pin, rise_fall ? "rising" : "falling", 0, 0, 0);
            }
        logMsg("Group %d is enabled, mode: %d\n", i, mode, 0, 0, 0, 0);              
        if (sample_buffer_addr[i] != NULL) { // 检查是否已分配内存
            free(sample_buffer_addr[i]); // 释放已有内存
        }
        sample_buffer_addr[i] = (uint32_t *)memalign(0x1000, data_size); // 分配内存
        if (sample_buffer_addr[i] == NULL) { // 检查是否分配成功
             logMsg("Memory allocation failed, index: %d\n", i, 0, 0, 0, 0, 0);
            // Free the already allocated memory
            for (j = 0; j < i; j++) {
                if (sample_buffer_addr[j] != NULL) {
                    free(sample_buffer_addr[j]);
                }
            }
            resp.status = 0xFF; 
            axiSrioSend(g_srioDev, &resp, sizeof(resp), 0);
            return ERROR; // 分配失败，返回错误
        }
         PHYS_ADDR    physicalAddr;
         if(vmTranslate (NULL,(VIRT_ADDR) sample_buffer_addr[i], &physicalAddr) == ERROR) {
             for (j = 0; j <= i; j++) {
                 if (sample_buffer_addr[j] != NULL) {
                     free(sample_buffer_addr[j]);
                 }
             }
            logMsg("vmTranslate failed for index %d, virtual address: 0x%x, free all allocated memory\n", i, (unsigned int)physicalAddr, 0, 0, 0, 0);
		  resp.status = 0xFF; 
		  axiSrioSend(g_srioDev, &resp, sizeof(resp), 0);
		  return ERROR; // 分配失败，返回错误
		}
		  else {
			 logMsg("vmTranslate success for index %d, virtual address: 0x%x, physical address: 0x%x\n", i, (unsigned int)sample_buffer_addr[i], (unsigned int)physicalAddr,  0, 0, 0);
			 set_buffer_address(i, (unsigned int)physicalAddr);
			 set_sample_duration(i, 200*40000*(time + 1));
			 enable_group(i,1); 
		  }
	  }
    resp.status = 0x0;  
    logMsg("Mode set response sent successfully.\n", 0, 0, 0, 0, 0, 0);

    axiSrioSend(g_srioDev, &resp, sizeof(resp), 0);
    return OK;
}

// 状态查询指令处理
STATUS srio_process_status_query(IcdPacket* packet) {
    uint8_t val = 0;
    StatusQueryResponse resp = {0};
    resp.header_flag = 0x5AA55AA5;
    resp.command_type = ICD_ACQ_CTRL;
    resp.tail_flag = 0xADDEADDE;    

    // 获取槽位号并填充到物理地址
    uint8_t slot_number = get_fpga_slot_number();
    resp.zynq_phy_addr = slot_number;
    uint8_t mode  = packet->payload[1] & 0xf; 
    if(mode == 0xf)
        syncControlAllGroups(1);
    else
        syncControlAllGroups(0);
    
    val = ((is_group_enabled(3) << 7) | (is_group_enabled(2) << 6) | (is_group_enabled(1) << 5) | (is_group_enabled(0) << 4)) | mode;
    resp.zynq_channel_status[0] =  val;
    
    val = (is_group_enabled(11) << 7)   | (is_group_enabled(10) << 6) | (is_group_enabled(9) << 5) | (is_group_enabled(8) << 4) | (is_group_enabled(7) << 3) | (is_group_enabled(6) << 2) | (is_group_enabled(5) << 1) | (is_group_enabled(4) << 0) ; 
    resp.zynq_channel_status[1] =  val;

    val = (is_group_enabled(19) << 7) | (is_group_enabled(18) << 6) | (is_group_enabled(17) << 5) | (is_group_enabled(16) << 4) | (is_group_enabled(15) << 3) | (is_group_enabled(14) << 2) | (is_group_enabled(13) << 1) | (is_group_enabled(12) << 0) ;                    
    resp.zynq_channel_status[2] =  val;



    // 填充组完成状态 - 参考zynq_channel_status的配置方式
    val = (is_group_finish(3) << 7) | (is_group_finish(2) << 6) | (is_group_finish(1) << 5) | (is_group_finish(0) << 4);
    resp.zynq_channel_completestatus[0] = val;
    logMsg("zynq_channel_completestatus[0] set to 0x%02x\n", val, 0, 0, 0, 0, 0);

    val = (is_group_finish(11) << 7) | (is_group_finish(10) << 6) | (is_group_finish(9) << 5) | (is_group_finish(8) << 4) | (is_group_finish(7) << 3) | (is_group_finish(6) << 2) | (is_group_finish(5) << 1) | (is_group_finish(4) << 0);
    resp.zynq_channel_completestatus[1] = val;
    logMsg("zynq_channel_completestatus[1] set to 0x%02x\n", val, 0, 0, 0, 0, 0);
    
    val = (is_group_finish(19) << 7) | (is_group_finish(18) << 6) | (is_group_finish(17) << 5) | (is_group_finish(16) << 4) | (is_group_finish(15) << 3) | (is_group_finish(14) << 2) | (is_group_finish(13) << 1) | (is_group_finish(12) << 0);
    resp.zynq_channel_completestatus[2] = val;
    logMsg("zynq_channel_completestatus[2] set to 0x%02x\n", val, 0, 0, 0, 0, 0);

    

    axiSrioSend(g_srioDev, &resp, sizeof(resp), 0);
    return OK;
}
/// 数据传输指令处理
STATUS srio_process_data_transfer(IcdPacket* packet) {
    static uint32_t group_enable_bits = 0;
    static uint32_t current_group = 0;
    static uint32_t packet_index = 0;
    static uint32_t total_packets = 0;
    static uint8_t *current_data_ptr = NULL;
    static BOOL is_new_transfer = TRUE;  // 新增标志位
    
    
    
    // 如果是新的传输指令
    if (packet->payload[1] == 0xFF) { // 开始传输
        // 解析组使能信息
        group_enable_bits = 0;
        group_enable_bits |= (uint32_t)packet->payload[2];        // 组0-7
        group_enable_bits |= (uint32_t)packet->payload[3] << 8;   // 组8-15
        group_enable_bits |= ((uint32_t)packet->payload[4] & 0x0F) << 16; // 组16-19
        
        // 初始化传输状态
        current_group = 0;
        packet_index = 0;
        current_data_ptr = NULL;
        is_new_transfer = TRUE;
        
        // 找到第一个使能的组
        while (current_group < 20 && !(group_enable_bits & (1U << current_group))) {
            current_group++;
        }
        
        if (current_group >= 20) {
            logMsg("No enabled groups found\n", 0, 0, 0, 0, 0, 0);
            return ERROR;
        }
        
        if (sample_buffer_addr[current_group] == NULL) {
            logMsg("Group %d buffer is NULL\n", current_group, 0, 0, 0, 0, 0);
            return ERROR;
        }
        
        current_data_ptr = (uint8_t *)sample_buffer_addr[current_group];
        total_packets = (data_size + MAX_PACKET_DATA_SIZE - 1) / MAX_PACKET_DATA_SIZE;
       
    }
    else if (packet->payload[1] == 0x00) { // 停止传输
        logMsg("Stopping data transfer\n", 0, 0, 0, 0, 0, 0);
        is_new_transfer = TRUE;  // 重置标志
        return OK;
    }

    // 如果不是新传输且没有数据，直接返回
    if (!is_new_transfer && current_data_ptr == NULL) {
        return ERROR;
    }
    

//    logMsg("Starting data transfer for group %d,index %d, total packets %d\n", 
//                 current_group, packet_index, total_packets, 0, 0, 0);
    // 准备数据包响应
    DataTransferResponse resp = {0};
    // 检查是否为最后一组最后一包
    int is_last_group = 0;
    uint32_t temp_group = current_group + 1;
    while (temp_group < 20 && !(group_enable_bits & (1U << temp_group))) {
        temp_group++;
    }
    if (temp_group >= 20) {
        is_last_group = 1;
    }

    if (is_last_group && packet_index == total_packets - 1) {
        resp.command_type = ICD_DATA_TRANSFER_RIOEND;
    } else {
        resp.command_type = ICD_DATA_TRANSFER_RIO;
    }
    resp.header_flag = 0x5AA55AA5;
    resp.channel = current_group;
    resp.packets_num = total_packets;
    resp.packet_index = packet_index;
    resp.zynq_phy_addr = get_fpga_slot_number();
    
    // 计算当前包的有效数据长度，最大为1000字节
    uint32_t remaining_data = data_size - packet_index * MAX_PACKET_DATA_SIZE;
    uint32_t packet_valid_len = (remaining_data < MAX_PACKET_DATA_SIZE) ? remaining_data : MAX_PACKET_DATA_SIZE;
    
    // 确保不超过data数组的实际大小（1000字节）
    if (packet_valid_len > PACKET_DATA_SIZE) {
        packet_valid_len = PACKET_DATA_SIZE;
    }
    
    // 填充数据，使用1000字节边界计算偏移
    if (current_data_ptr != NULL) {
       memcpy(resp.data, current_data_ptr + packet_index * MAX_PACKET_DATA_SIZE, packet_valid_len);
    }
    resp.packet_valid_len = packet_valid_len;
    resp.tail_flag = 0xADDEADDE;
    
    // 发送当前数据包
    STATUS status = axiSrioSend(g_srioDev, &resp, sizeof(resp), 0);
    if (status != OK) {
        return status;
    }
    if(packet_index == 0)        
    {
        logMsg("group_enable_bits: 0x%08x\n", group_enable_bits, 0, 0, 0, 0, 0);
        logMsg("Starting data transfer for group %d, total packets %d\n", 
            current_group, total_packets, 0, 0, 0, 0);
    }
    // 更新索引
    packet_index++;
    is_new_transfer = FALSE;  // 标记为正在传输中
    if (packet_index >= total_packets) {
        // 当前组传输完成，找下一个使能的组
        packet_index = 0;
        current_group++;
        
        while (current_group < 20 && !(group_enable_bits & (1U << current_group))) {
            current_group++;
        }
        
        if (current_group < 20) {
            if (sample_buffer_addr[current_group] != NULL) {
                current_data_ptr = (uint8_t *)sample_buffer_addr[current_group];
                total_packets = (data_size + MAX_PACKET_DATA_SIZE - 1) / MAX_PACKET_DATA_SIZE;
             
            } else {
                logMsg("Group %d buffer is NULL\n", current_group, 0, 0, 0, 0, 0);
                return ERROR;
            }
        } else {
            // 所有组传输完成
            logMsg("All groups data transfer completed\n", 0, 0, 0, 0, 0, 0);
            is_new_transfer = TRUE;  // 重置标志
            return OK;
        }
    }
    
    return OK;
  
}
// SRIO接收任务
static void srio_receive_task(void) {
    logMsg("srio_receive_task create completed\n", 0, 0, 0, 0, 0, 0);

	
    while (1) {
        if (semTake(g_srioSem, WAIT_FOREVER) == OK) {
            // 获取接收数据地址
            void* rxAddr = axiSrioGetRxAddr(g_srioDev);
            if (rxAddr != NULL) {
                IcdPacket* packet = (IcdPacket*)rxAddr;
                
                // 验证包头
                if (packet->header_flag != 0x5AA55AA5) {
                    continue;
                }
                
                // 处理命令
                switch (packet->command_type) {
                    case ICD_HANDSHAKE:
                        logMsg("Processing ICD_HANDSHAKE command\n", 0, 0, 0, 0, 0, 0);
                        srio_process_handshake(packet);
                        break;
                    case ICD_MODE_SET:
                        logMsg("Processing ICD_MODE_SET command\n", 0, 0, 0, 0, 0, 0);
                        srio_process_mode_set(packet);
                        break;
                    case ICD_ACQ_CTRL:
                        logMsg("Processing ICD_ACQ_CTRL command\n", 0, 0, 0, 0, 0, 0);
                        srio_process_status_query(packet);
                        break;
                    case ICD_DATA_TRANSFER:
                        srio_process_data_transfer(packet);
                        break;
                    default:
                        logMsg("Unknown SRIO command: %d\n", packet->command_type, 0, 0, 0, 0, 0);
                        break;
                }
            }
        }
    }
}


 
